#include "kalman_2state.h"

kalman_2state::kalman_2state()
{

}

kalman_2state::kalman_2state(boost::numeric::ublas::matrix<double> _R,
							 boost::numeric::ublas::matrix<double> _Q,
							 double dt):
	A(2,2),
    H(1,2),
	S(1,1),
	K(1,1),
	R(1,1),
	Z(1,1),
	Inn(1,1)
{
	//Transition Matrix
	A(0,0) = 1;
	A(0,1) = dt;
	A(1,0) = 0;
	A(1,1) = 1;

	//Obervation Matrix, Observe on position
	H(0,0) = 1;
	H(0,1) = 0;

	Q = _Q;
	R = _R;
	Inn = _R;

}


void kalman_2state::update(boost::numeric::ublas::matrix<double> &_state,
						   boost::numeric::ublas::matrix<double> &_state_estimate,
						   boost::numeric::ublas::matrix<double> &_covariance,
						   boost::numeric::ublas::matrix<double> _Z,
						   boost::numeric::ublas::matrix<double> _ProcessNoise)
{
	//Update The State
	_state = prod( A , _state ) + _ProcessNoise;
	
	//Innovation
	Inn = _Z - prod( H ,  _state_estimate );

	boost::numeric::ublas::matrix<double> P_Ht(2,1);
	P_Ht = prod( _covariance , trans(H) );

	//S = HPHt + R
	S = prod( H , P_Ht );
	S = S + R;

	//Kalman Gain
	K = prod( A , P_Ht );
	K = K / S(0,0);

	//Update the State Estimate
	_state_estimate = prod( A , _state_estimate ) + prod( K , Inn );



	boost::numeric::ublas::matrix<double> P_At(2,2);
	P_At = prod(_covariance,trans(A));

	boost::numeric::ublas::matrix<double> H_P_At(1,2);
	H_P_At = prod( H ,P_At);

	boost::numeric::ublas::matrix<double> P_Ht_H_P_At(2,2);
	P_Ht_H_P_At = prod ( P_Ht, H_P_At ) ;

	boost::numeric::ublas::matrix<double> A_P_Ht_H_P_At(2,2);
	A_P_Ht_H_P_At  = prod( A , P_Ht_H_P_At );

	//Update the Covariance
	_covariance = prod( A , P_At ) + Q - A_P_Ht_H_P_At / S(0,0);
};